package tests;
import com.neuronrobotics.sdk.dyio.DyIO;
import com.neuronrobotics.sdk.dyio.peripherals.AnalogInputChannel;
import com.neuronrobotics.sdk.dyio.peripherals.ServoChannel;
import com.neuronrobotics.sdk.ui.ConnectionDialog;
public class PID {
	public static void main(String[] args) throws InterruptedException {
		DyIO dyio=new DyIO();
		if (!ConnectionDialog.getBowlerDevice(dyio)){
			System.exit(1);
		}
		AnalogInputChannel ir_right = new AnalogInputChannel (dyio.getChannel(10)); //  right infrared 10
		AnalogInputChannel ir_left = new AnalogInputChannel (dyio.getChannel(11)); // left infrared port 11
		AnalogInputChannel us_left = new AnalogInputChannel (dyio.getChannel(9)); // left ultra sonic port 9
		AnalogInputChannel us_right = new AnalogInputChannel (dyio.getChannel(8)); // right ultra sonic port 8
		ServoChannel srv_left = new ServoChannel (dyio.getChannel(12)); //left wheel port 12
		ServoChannel srv_right = new ServoChannel (dyio.getChannel(13)); // right wheel port 13
		
		int mode = 16;
		{
{
	while(true)
	{
	switch(mode)
	{
	case 16:		
	System.out.println("Running Case 16 = 1111"); // tells me each sensor is on
	Thread.sleep(100);
	int A = 0; //(int) ((1689/64)/( ir_left.getVoltage()));  // Alinear = ((1689/64)./((0)+A)); 
	int B = 0; //(int) ((6428/256)/( ir_right.getVoltage()));  // Blinear = ((6428/256)./((0)+B));
	int C = 0; //(int) ( 4 + ((102.6)*( us_left.getVoltage())) ) ;  // Clinear = ((102.6)*C) + 4;
	int D = 0;//(int)  (3.7899999 + (101.3*( us_right.getVoltage()))); // Dlinear = 3.7899999+ ((101.3)*D);
	int error3 = 0;
	int pe3 = 0; 
	int pe3_1 = 0;
	int pe3_2 = 0;
	int pe3_3 = 0;
	int pe3_4 = 0;	
	int error4 = 0;
	int pe4 = 0;
	int pe4_1 = 0;
	int pe4_2 = 0;
	int pe4_3 = 0;
	int pe4_4 = 0;
	int error5 = 0;
	int pe5 = 0;
	int pe5_1 = 0;
	int pe5_2 = 0;
	int pe5_3 = 0;
	int pe5_4 = 0;
	int error6 = 0;
	int pe6 = 0;
	int pe6_1 = 0;
	int pe6_2 = 0;
	int pe6_3 = 0;
	int pe6_4 = 0;
	for( int i = 0; i<5; i++)
	{	
		A = (int) ((1689/64)/( ir_left.getVoltage()));  // Alinear = ((1689/64)./((0)+A)); 
		B = (int) ((6428/256)/( ir_right.getVoltage()));  // Blinear = ((6428/256)./((0)+B));
		C = (int) ( 4 + ((102.6)*( us_left.getVoltage())) ) ;  // Clinear = ((102.6)*C) + 4;
		D = (int)  (3.7899999 + (101.3*( us_right.getVoltage()))); // Dlinear = 3.7899999+ ((101.3)*D);		
	// Rather than subtracting from each other I thought subtracting from
	// 20 would be a good idea because then it would have to line up
	// 20 inches away from the wall always.
	error3 = (int) (20-A);
	error4 = (int) (20-B);
	error5 = (int) (20-C);
	error6 = (int) (20-D);	
	// keeps track of a few error values
	 pe3_4 = pe3_3;
	 pe3_3 = pe3_2;
	 pe3_2 = pe3_1;
	 pe3_1 = pe3;
	 pe3 = error3; 
	 pe4_4 = pe4_3;
	 pe4_3 = pe4_2;
	 pe4_2 = pe4_1;
	 pe4_1 = pe4;
	 pe4 = error4; 
	 pe5_4 = pe5_3;
	 pe5_3 = pe5_2;
	 pe5_2 = pe5_1;
	 pe5_1 = pe5;
	 pe5 = error5; 
	 pe6_4 = pe6_3;
	 pe6_3 = pe6_2;
	 pe6_2 = pe6_1;
	 pe6_1 = pe6;
	 pe6 = error6;
	}
	// Averages the error values from the stored readings
	
	int E3 = (int) ((pe3 + pe3_1 + pe3_2 + pe3_3 + pe3_4)/5);
	int E4 = (int) ((pe4 + pe4_1 + pe4_2 + pe4_3 + pe4_4)/5);
	int E5 = (int) ((pe5 + pe5_1 + pe5_2 + pe5_3 + pe5_4)/5);
	int E6 = (int) ((pe6 + pe6_1 + pe6_2 + pe6_3 + pe6_4)/5);
	
	// Averages errors from the left side and right side
	int EL = ((E3 + E5)/2);
	int ER = ((E4 + E6)/2);
	int kp = 17 ; // random constant for the p controller
	// The P controller is the stop speed (128) + or - the 
	// product of the error and random constant
int SR =  (int) (128 -(kp*ER)); // 118 is still frozen?
int SL =  (int) (128 +(kp*EL)); // 136 is still frozen
//these are to keep the speeds at acceptable values 0 - 255
	if (SL > 255) SL = 255; 
	if (SL < 0) SL = 0;
	if (SR > 255) SR = 255; 
	if (SR < 0) SR = 0;
	srv_left.SetPosition(SL, 1); 
	srv_right.SetPosition(SR, 1);
	Thread.sleep(250);
	System.out.println(E3+"	"+ E4+"	"+E5+"	"+ E6);
	srv_left.SetPosition(128, 1); 
	srv_right.SetPosition(128, 1);
	
	if((D < B-3)|| (D > B+3)) mode = 15;//1110
	
	break;
	
 case 15: //( if D breaks)
		System.out.println("Running Case 15 = 1110"); // this tells me D is down
		Thread.sleep(100);
	 
	int aA = 0; //(int) ((1689/64)/( ir_left.getVoltage()));  // Alinear = ((1689/64)./((0)+A)); 
	int aB = 0; //(int) ((6428/256)/( ir_right.getVoltage()));  // Blinear = ((6428/256)./((0)+B));
	int aC = 0; //(int) ( 4 + ((102.6)*( us_left.getVoltage())) ) ;  // Clinear = ((102.6)*C) + 4;

	int aerror3 = 0;
	int ape3 = 0; 
	int ape3_1 = 0;
	int ape3_2 = 0;
	int ape3_3 = 0;
	int ape3_4 = 0;	
	int aerror4 = 0;
	int ape4 = 0;
	int ape4_1 = 0;
	int ape4_2 = 0;
	int ape4_3 = 0;
	int ape4_4 = 0;
	int aerror5 = 0;
	int ape5 = 0;
	int ape5_1 = 0;
	int ape5_2 = 0;
	int ape5_3 = 0;
	int ape5_4 = 0;

	for( int i = 0; i<5; i++)
	{	
		aA = (int) ((1689/64)/( ir_left.getVoltage()));  // Alinear = ((1689/64)./((0)+A)); 
		aB = (int) ((6428/256)/( ir_right.getVoltage()));  // Blinear = ((6428/256)./((0)+B));
		aC = (int) ( 4 + ((102.6)*( us_left.getVoltage())) ) ;  // Clinear = ((102.6)*C) + 4;
	
	// Rather than subtracting from each other I thought subtracting from
	// 20 would be a good idea because then it would have to line up
	// 20 inches away from the wall always.
	aerror3 = (int) (20-aA);
	aerror4 = (int) (20-aB);
	aerror5 = (int) (20-aC);
	
	// keeps track of a few error values
	 ape3_4 = ape3_3;
	 ape3_3 = ape3_2;
	 ape3_2 = ape3_1;
	 ape3_1 = ape3;
	 ape3 = aerror3; 
	 ape4_4 = ape4_3;
	 ape4_3 = ape4_2;
	 ape4_2 = ape4_1;
	 ape4_1 = ape4;
	 ape4 = aerror4; 
	 ape5_4 = ape5_3;
	 ape5_3 = ape5_2;
	 ape5_2 = ape5_1;
	 ape5_1 = ape5;
	 ape5 = aerror5; 

	}
	// Averages the error values from the stored readings
	int aE3 = (int) ((ape3 + ape3_1 + ape3_2 + ape3_3 + ape3_4)/5);
	int aE4 = (int) ((ape4 + ape4_1 + ape4_2 + ape4_3 + ape4_4)/5);
	int aE5 = (int) ((ape5 + ape5_1 + ape5_2 + ape5_3 + ape5_4)/5);

	// Averages errors from the left side and right side
	int aEL = ((aE3 + aE5)/2);
	int aER = aE4;
	int akp = 17 ; // random constant for the p controller
	// The P controller is the stop speed (128) + or - the 
	// product of the error and random constant
int aSR =  (int) (128 -(akp*aER)); // 118 is still frozen?
int aSL =  (int) (128 +(akp*aEL)); // 136 is still frozen
//these are to keep the speeds at acceptable values 0 - 255
	if (aSL > 255) aSL = 255; 
	if (aSL < 0) aSL = 0;
	if (aSR > 255) aSR = 255; 
	if (aSR < 0) aSR = 0;
	srv_left.SetPosition(aSL, 1); 
	srv_right.SetPosition(aSR, 1);
	Thread.sleep(250);
	System.out.println(aE3+"	"+ aE4+"	"+aE5+"	"+ "broken");
	srv_left.SetPosition(128, 1); 
	srv_right.SetPosition(128, 1);
 break;
	


 case 13: // IF C breaks
	System.out.println("Running Case 13 = 1101"); // tells me C is down
	Thread.sleep(100);

 	int bA = 0; //(int) ((1689/64)/( ir_left.getVoltage()));  // Alinear = ((1689/64)./((0)+A)); 
	int bB = 0; //(int) ((6428/256)/( ir_right.getVoltage()));  // Blinear = ((6428/256)./((0)+B));
	
	int bD = 0;//(int)  (3.7899999 + (101.3*( us_right.getVoltage()))); // Dlinear = 3.7899999+ ((101.3)*D);
	int berror3 = 0; // A
	int bpe3 = 0; 
	int bpe3_1 = 0;
	int bpe3_2 = 0;
	int bpe3_3 = 0;
	int bpe3_4 = 0;	
	int berror4 = 0; // B
	int bpe4 = 0;
	int bpe4_1 = 0;
	int bpe4_2 = 0;
	int bpe4_3 = 0;
	int bpe4_4 = 0;

	int berror6 = 0; // D
	int bpe6 = 0;
	int bpe6_1 = 0;
	int bpe6_2 = 0;
	int bpe6_3 = 0;
	int bpe6_4 = 0;
	for( int i = 0; i<5; i++)
	{	
		bA = (int) ((1689/64)/( ir_left.getVoltage()));  // Alinear = ((1689/64)./((0)+A)); 
		bB = (int) ((6428/256)/( ir_right.getVoltage()));  // Blinear = ((6428/256)./((0)+B));

		bD = (int)  (3.7899999 + (101.3*( us_right.getVoltage()))); // Dlinear = 3.7899999+ ((101.3)*D);		
	// Rather than subtracting from each other I thought subtracting from
	// 20 would be a good idea because then it would have to line up
	// 20 inches away from the wall always.
	berror3 = (int) (20-bA);
	berror4 = (int) (20-bB);

	berror6 = (int) (20-bD);	
	// keeps track of a few error values
	 bpe3_4 = bpe3_3;
	 bpe3_3 = bpe3_2;
	 bpe3_2 = bpe3_1;
	 bpe3_1 = bpe3;
	 bpe3 = berror3; 
	 bpe4_4 = bpe4_3;
	 bpe4_3 = bpe4_2;
	 bpe4_2 = bpe4_1;
	 bpe4_1 = bpe4;
	 bpe4 = berror4; 

	 bpe6_4 = bpe6_3;
	 bpe6_3 = bpe6_2;
	 bpe6_2 = bpe6_1;
	 bpe6_1 = bpe6;
	 bpe6 = berror6;
	}
	// Averages the error values from the stored readings
	int bE3 = (int) ((bpe3 + bpe3_1 + bpe3_2 + bpe3_3 + bpe3_4)/5);
	int bE4 = (int) ((bpe4 + bpe4_1 + bpe4_2 + bpe4_3 + bpe4_4)/5);

	int bE6 = (int) ((bpe6 + bpe6_1 + bpe6_2 + bpe6_3 + bpe6_4)/5);
	// Averages errors from the left side and right side
	int bEL = bE3;
	int bER = ((bE4 + bE6)/2);
	int bkp = 16 ; // random constant for the p controller
	// The P controller is the stop speed (128) + or - the 
	// product of the error and random constant
int bSR =  (int) (128 -(bkp*bER)); // 118 is still frozen?
int bSL =  (int) (128 +(bkp*bEL)); // 136 is still frozen
//these are to keep the speeds at acceptable values 0 - 255
	if (bSL > 255) bSL = 255; 
	if (bSL < 0) bSL = 0;
	if (bSR > 255) bSR = 255; 
	if (bSR < 0) bSR = 0;
	srv_left.SetPosition(bSL, 1); 
	srv_right.SetPosition(bSR, 1);
	Thread.sleep(100);
	System.out.println(bE3+"	"+ bE4+"	"+"broken"+"	"+ bE6);
	srv_left.SetPosition(128, 1); 
	srv_right.SetPosition(128, 1);
	break;
 

 case 11: // If B breaks
	System.out.println("Running Case 11 = 1011"); // tells me B is down
	Thread.sleep(100);

  	int cA = 0; //(int) ((1689/64)/( ir_left.getVoltage()));  // Alinear = ((1689/64)./((0)+A)); 

	int cC = 0; //(int) ( 4 + ((102.6)*( us_left.getVoltage())) ) ;  // Clinear = ((102.6)*C) + 4;
	int cD = 0;//(int)  (3.7899999 + (101.3*( us_right.getVoltage()))); // Dlinear = 3.7899999+ ((101.3)*D);

	int cerror3 = 0; // A
	int cpe3 = 0; 
	int cpe3_1 = 0;
	int cpe3_2 = 0;
	int cpe3_3 = 0;
	int cpe3_4 = 0;	

	int cerror5 = 0; // C
	int cpe5 = 0;
	int cpe5_1 = 0;
	int cpe5_2 = 0;
	int cpe5_3 = 0;
	int cpe5_4 = 0;
	int cerror6 = 0; // D
	int cpe6 = 0;
	int cpe6_1 = 0;
	int cpe6_2 = 0;
	int cpe6_3 = 0;
	int cpe6_4 = 0;
	for( int i = 0; i<5; i++)
	{	
		cA = (int) ((1689/64)/( ir_left.getVoltage()));  // Alinear = ((1689/64)./((0)+A)); 
		
		cC = (int) ( 4 + ((102.6)*( us_left.getVoltage())) ) ;  // Clinear = ((102.6)*C) + 4;
		cD = (int)  (3.7899999 + (101.3*( us_right.getVoltage()))); // Dlinear = 3.7899999+ ((101.3)*D);		
	// Rather than subtracting from each other I thought subtracting from
	// 20 would be a good idea because then it would have to line up
	// 20 inches away from the wall always.
	cerror3 = (int) (20-cA);

	cerror5 = (int) (20-cC);
	cerror6 = (int) (20-cD);	
	// keeps track of a few error values
	 cpe3_4 = cpe3_3;
	 cpe3_3 = cpe3_2;
	 cpe3_2 = cpe3_1; 
	 cpe3_1 = cpe3;
	 cpe3 = cerror3; 

	 cpe5_4 = cpe5_3;
	 cpe5_3 = cpe5_2;
	 cpe5_2 = cpe5_1;
	 cpe5_1 = cpe5;
	 cpe5 = cerror5; 
	 cpe6_4 = cpe6_3;
	 cpe6_3 = cpe6_2;
	 cpe6_2 = cpe6_1;
	 cpe6_1 = cpe6;
	 cpe6 = cerror6;
	}
	// Averages the error values from the stored readings
	int cE3 = (int) ((cpe3 + cpe3_1 + cpe3_2 + cpe3_3 + cpe3_4)/5);

	int cE5 = (int) ((cpe5 + cpe5_1 + cpe5_2 + cpe5_3 + cpe5_4)/5);
	int cE6 = (int) ((cpe6 + cpe6_1 + cpe6_2 + cpe6_3 + cpe6_4)/5);
	// Averages errors from the left side and right side
	int cEL = ((cE3 + cE5)/2);
	int cER = cE6;
	int ckp = 17 ; // random constant for the p controller
	// The P controller is the stop speed (128) + or - the 
	// product of the error and random constant
int cSR =  (int) (128 -(ckp*cER)); // 118 is still frozen?
int cSL =  (int) (128 +(ckp*cEL)); // 136 is still frozen
//these are to keep the speeds at acceptable values 0 - 255
	if (cSL > 255) cSL = 255; 
	if (cSL < 0) cSL = 0;
	if (cSR > 255) cSR = 255; 
	if (cSR < 0) cSR = 0;
	srv_left.SetPosition(cSL, 1); 
	srv_right.SetPosition(cSR, 1);
	Thread.sleep(250);
	System.out.println(cE3+"	"+"broken" +"	"+cE5+"	"+ cE6);
	srv_left.SetPosition(128, 1); 
	srv_right.SetPosition(128, 1);
	break;


 case 7: //if A breaks
 
 	System.out.println("Running Case 7 = 0111"); // tells me A is down
	Thread.sleep(100);

	int dB = 0; //(int) ((6428/256)/( ir_right.getVoltage()));  // Blinear = ((6428/256)./((0)+B));
	int dC = 0; //(int) ( 4 + ((102.6)*( us_left.getVoltage())) ) ;  // Clinear = ((102.6)*C) + 4;
	int dD = 0;//(int)  (3.7899999 + (101.3*( us_right.getVoltage()))); // Dlinear = 3.7899999+ ((101.3)*D);
	
	int derror4 = 0; // B
	int dpe4 = 0;
	int dpe4_1 = 0;
	int dpe4_2 = 0;
	int dpe4_3 = 0;
	int dpe4_4 = 0;
	int derror5 = 0; // C
	int dpe5 = 0;
	int dpe5_1 = 0;
	int dpe5_2 = 0;
	int dpe5_3 = 0;
	int dpe5_4 = 0;
	int derror6 = 0; // D
	int dpe6 = 0;
	int dpe6_1 = 0;
	int dpe6_2 = 0;
	int dpe6_3 = 0;
	int dpe6_4 = 0;
	for( int i = 0; i<5; i++)
	{	
		
		dB = (int) ((6428/256)/( ir_right.getVoltage()));  // Blinear = ((6428/256)./((0)+B));
		dC = (int) ( 4 + ((102.6)*( us_left.getVoltage())) ) ;  // Clinear = ((102.6)*C) + 4;
		dD = (int)  (3.7899999 + (101.3*( us_right.getVoltage()))); // Dlinear = 3.7899999+ ((101.3)*D);		
	// Rather than subtracting from each other I thought subtracting from
	// 20 would be a good idea because then it would have to line up
	// 20 inches away from the wall always.
	
	derror4 = (int) (20-dB);
	derror5 = (int) (20-dC);
	derror6 = (int) (20-dD);	
	// keeps track of a few error values
 
	 dpe4_4 = dpe4_3;
	 dpe4_3 = dpe4_2;
	 dpe4_2 = dpe4_1;
	 dpe4_1 = dpe4;
	 dpe4 = derror4; 
	 dpe5_4 = dpe5_3;
	 dpe5_3 = dpe5_2;
	 dpe5_2 = dpe5_1;
	 dpe5_1 = dpe5;
	 dpe5 = derror5; 
	 dpe6_4 = dpe6_3;
	 dpe6_3 = dpe6_2;
	 dpe6_2 = dpe6_1;
	 dpe6_1 = dpe6;
	 dpe6 = derror6;
	}
	// Averages the error values from the stored readings
	
	int dE4 = (int) ((dpe4 + dpe4_1 + dpe4_2 + dpe4_3 + dpe4_4)/5);
	int dE5 = (int) ((dpe5 + dpe5_1 + dpe5_2 + dpe5_3 + dpe5_4)/5);
	int dE6 = (int) ((dpe6 + dpe6_1 + dpe6_2 + dpe6_3 + dpe6_4)/5);
	// Averages errors from the left side and right side
	int dEL = dE5;
	int dER = ((dE4 + dE6)/2);
	int dkp = 15 ; // random constant for the p controller
	// The P controller is the stop speed (128) + or - the 
	// product of the error and random constant
int dSR =  (int) (128 -(dkp*dER)); // 118 is still frozen?
int dSL =  (int) (128 +(dkp*dEL)); // 136 is still frozen
//these are to keep the speeds at acceptable values 0 - 255
	if (dSL > 255) dSL = 255; 
	if (dSL < 0) dSL = 0;
	if (dSR > 255) dSR = 255; 
	if (dSR < 0) dSR = 0;
	srv_left.SetPosition(dSL, 1); 
	srv_right.SetPosition(dSR, 1);
	Thread.sleep(250);
	System.out.println("broken"+"	"+ dE4+"	"+dE5+"	"+ dE6);
	srv_left.SetPosition(128, 1); 
	srv_right.SetPosition(128, 1);
	break;



 case 12: //( if D and C break) (no more Ultra sonics)
 	System.out.println("Running Case 12 = 1100"); // tells me C and D are down
	Thread.sleep(100);

	int eA = 0; //(int) ((1689/64)/( ir_left.getVoltage()));  // Alinear = ((1689/64)./((0)+A)); 
	int eB = 0; //(int) ((6428/256)/( ir_right.getVoltage()));  // Blinear = ((6428/256)./((0)+B));
	
	int eerror3 = 0;
	int epe3 = 0; 
	int epe3_1 = 0;
	int epe3_2 = 0;
	int epe3_3 = 0;
	int epe3_4 = 0;	
	int eerror4 = 0;
	int epe4 = 0;
	int epe4_1 = 0;
	int epe4_2 = 0;
	int epe4_3 = 0;
	int epe4_4 = 0;
	for( int i = 0; i<5; i++)
	{	
		eA = (int) ((1689/64)/( ir_left.getVoltage()));  // Alinear = ((1689/64)./((0)+A)); 
		eB = (int) ((6428/256)/( ir_right.getVoltage()));  // Blinear = ((6428/256)./((0)+B));
	
	// Rather than subtracting from each other I thought subtracting from
	// 20 would be a good idea because then it would have to line up
	// 20 inches away from the wall always.
	eerror3 = (int) (20-eA);
	eerror4 = (int) (20-eB);
	
	
	// keeps track of a few error values
	 epe3_4 = epe3_3;
	 epe3_3 = epe3_2;
	 epe3_2 = epe3_1;
	 epe3_1 = epe3;
	 epe3 = eerror3; 
	 epe4_4 = epe4_3;
	 epe4_3 = epe4_2;
	 epe4_2 = epe4_1;
	 epe4_1 = epe4;
	 epe4 = eerror4; 


	}
	// Averages the error values from the stored readings
	int eE3 = (int) ((epe3 + epe3_1 + epe3_2 + epe3_3 + epe3_4)/5);
	int eE4 = (int) ((epe4 + epe4_1 + epe4_2 + epe4_3 + epe4_4)/5);

	// Averages errors from the left side and right side
	int eEL = eE3;
	int eER = eE4;
	int ekp = 17 ; // random constant for the p controller
	// The P controller is the stop speed (128) + or - the 
	// product of the error and random constant
int eSR =  (int) (128 -(ekp*eER)); // 118 is still frozen?
int eSL =  (int) (128 +(ekp*eEL)); // 136 is still frozen
//these are to keep the speeds at acceptable values 0 - 255
	if (eSL > 255) eSL = 255; 
	if (eSL < 0) eSL = 0;
	if (eSR > 255) eSR = 255; 
	if (eSR < 0) eSR = 0;
	srv_left.SetPosition(eSL, 1); 
	srv_right.SetPosition(eSR, 1);
	Thread.sleep(100);
	System.out.println(eE3+"	"+ eE4+"	"+"broken"+"	"+ "broken");
	srv_left.SetPosition(128, 1); 
	srv_right.SetPosition(128, 1);
	break;



 case 3: // if A and B break (No IRs)
 	System.out.println("Running Case 3 = 0011"); // tells me A and B are down
	Thread.sleep(100);

	int fC = 0; //(int) ( 4 + ((102.6)*( us_left.getVoltage())) ) ;  // Clinear = ((102.6)*C) + 4;
	int fD = 0;//(int)  (3.7899999 + (101.3*( us_right.getVoltage()))); // Dlinear = 3.7899999+ ((101.3)*D);
	
	int ferror5 = 0; // C
	int fpe5 = 0;
	int fpe5_1 = 0;
	int fpe5_2 = 0;
	int fpe5_3 = 0;
	int fpe5_4 = 0;
	int ferror6 = 0; // D
	int fpe6 = 0;
	int fpe6_1 = 0;
	int fpe6_2 = 0;
	int fpe6_3 = 0;
	int fpe6_4 = 0;
	for( int i = 0; i<5; i++)
	{	
		
		fC = (int) ( 4 + ((102.6)*( us_left.getVoltage())) ) ;  // Clinear = ((102.6)*C) + 4;
		fD = (int)  (3.7899999 + (101.3*( us_right.getVoltage()))); // Dlinear = 3.7899999+ ((101.3)*D);		
	// Rather than subtracting from each other I thought subtracting from
	// 20 would be a good idea because then it would have to line up
	// 20 inches away from the wall always.
	
	ferror5 = (int) (20-fC);
	ferror6 = (int) (20-fD);	
	// keeps track of a few error values
  
	 fpe5_4 = fpe5_3;
	 fpe5_3 = fpe5_2;
	 fpe5_2 = fpe5_1;
	 fpe5_1 = fpe5;
	 fpe5 = ferror5; 
	 fpe6_4 = fpe6_3;
	 fpe6_3 = fpe6_2;
	 fpe6_2 = fpe6_1;
	 fpe6_1 = fpe6;
	 fpe6 = ferror6;
	}
	// Averages the error values from the stored readings
	

	int fE5 = (int) ((fpe5 + fpe5_1 + fpe5_2 + fpe5_3 + fpe5_4)/5);
	int fE6 = (int) ((fpe6 + fpe6_1 + fpe6_2 + fpe6_3 + fpe6_4)/5);
	// Averages errors from the left side and right side
	int fEL = fE5;
	int fER = fE6;
	int fkp = 17 ; // random constant for the p controller
	// The P controller is the stop speed (128) + or - the 
	// product of the error and random constant
int fSR =  (int) (128 -(fkp*fER)); // 118 is still frozen?
int fSL =  (int) (128 +(fkp*fEL)); // 136 is still frozen
//these are to keep the speeds at acceptable values 0 - 255
	if (fSL > 255) fSL = 255; 
	if (fSL < 0) fSL = 0;
	if (fSR > 255) fSR = 255; 
	if (fSR < 0) fSR = 0;
	srv_left.SetPosition(fSL, 1); 
	srv_right.SetPosition(fSR, 1);
	Thread.sleep(100);
	System.out.println("broken"+"	"+ "broken"+"	"+fE5+"	"+ fE6);
	srv_left.SetPosition(128, 1); 
	srv_right.SetPosition(128, 1);
	break;


 case 6: // if A and D break
 	System.out.println("Running Case 6 = 0110"); // tells me A and D are down
	Thread.sleep(100);
	
	int gB = 0; //(int) ((6428/256)/( ir_right.getVoltage()));  // Blinear = ((6428/256)./((0)+B));
	int gC = 0; //(int) ( 4 + ((102.6)*( us_left.getVoltage())) ) ;  // Clinear = ((102.6)*C) + 4;

	int gerror4 = 0; // B
	int gpe4 = 0;
	int gpe4_1 = 0;
	int gpe4_2 = 0;
	int gpe4_3 = 0;
	int gpe4_4 = 0;
	int gerror5 = 0; // C
	int gpe5 = 0;
	int gpe5_1 = 0;
	int gpe5_2 = 0;
	int gpe5_3 = 0;
	int gpe5_4 = 0;

	for( int i = 0; i<5; i++)
	{	
		
		gB = (int) ((6428/256)/( ir_right.getVoltage()));  // Blinear = ((6428/256)./((0)+B));
		gC = (int) ( 4 + ((102.6)*( us_left.getVoltage())) ) ;  // Clinear = ((102.6)*C) + 4;
	
	// Rather than subtracting from each other I thought subtracting from
	// 20 would be a good idea because then it would have to line up
	// 20 inches away from the wall always.
	
	gerror4 = (int) (20-gB);
	gerror5 = (int) (20-gC);
	
	// keeps track of a few error values
 
	 gpe4_4 = gpe4_3;
	 gpe4_3 = gpe4_2;
	 gpe4_2 = gpe4_1;
	 gpe4_1 = gpe4;
	 gpe4 = gerror4; 
	 gpe5_4 = gpe5_3;
	 gpe5_3 = gpe5_2;
	 gpe5_2 = gpe5_1;
	 gpe5_1 = gpe5;
	 gpe5 = gerror5; 

	}
	// Averages the error values from the stored readings
	
	int gE4 = (int) ((gpe4 + gpe4_1 + gpe4_2 + gpe4_3 + gpe4_4)/5);
	int gE5 = (int) ((gpe5 + gpe5_1 + gpe5_2 + gpe5_3 + gpe5_4)/5);

	// Averages errors from the left side and right side
	int gEL = gE5;
	int gER = gE4;
	int gkp = 17 ; // random constant for the p controller
	// The P controller is the stop speed (128) + or - the 
	// product of the error and random constant
int gSR =  (int) (128 -(gkp*gER)); // 118 is still frozen?
int gSL =  (int) (128 +(gkp*gEL)); // 136 is still frozen
//these are to keep the speeds at acceptable values 0 - 255
	if (gSL > 255) gSL = 255; 
	if (gSL < 0) gSL = 0;
	if (gSR > 255) gSR = 255; 
	if (gSR < 0) gSR = 0;
	srv_left.SetPosition(gSL, 1); 
	srv_right.SetPosition(gSR, 1);
	Thread.sleep(100);
	System.out.println("broken"+"	"+ gE4+"	"+gE5+"	"+ "broken");
	srv_left.SetPosition(128, 1); 
	srv_right.SetPosition(128, 1);
	break;


 case 9: // if B and C break 

 	System.out.println("Running Case 9 = 1001"); // tells me C and B are down
	Thread.sleep(100);
	
  	int hA = 0; //(int) ((1689/64)/( ir_left.getVoltage()));  // Alinear = ((1689/64)./((0)+A)); 

	int hD = 0;//(int)  (3.7899999 + (101.3*( us_right.getVoltage()))); // Dlinear = 3.7899999+ ((101.3)*D);
	int herror3 = 0; // A
	int hpe3 = 0; 
	int hpe3_1 = 0;
	int hpe3_2 = 0;
	int hpe3_3 = 0;
	int hpe3_4 = 0;	

	int herror6 = 0; // D
	int hpe6 = 0;
	int hpe6_1 = 0;
	int hpe6_2 = 0;
	int hpe6_3 = 0;
	int hpe6_4 = 0;
	for( int i = 0; i<5; i++)
	{	
		hA = (int) ((1689/64)/( ir_left.getVoltage()));  // Alinear = ((1689/64)./((0)+A)); 
		
		hD = (int)  (3.7899999 + (101.3*( us_right.getVoltage()))); // Dlinear = 3.7899999+ ((101.3)*D);		
	// Rather than subtracting from each other I thought subtracting from
	// 20 would be a good idea because then it would have to line up
	// 20 inches away from the wall always.
	herror3 = (int) (20-hA);

	herror6 = (int) (20-hD);	
	// keeps track of a few error values
	 hpe3_4 = hpe3_3;
	 hpe3_3 = hpe3_2;
	 hpe3_2 = hpe3_1;
	 hpe3_1 = hpe3;
	 hpe3 = herror3; 
 
	 hpe6_4 = hpe6_3;
	 hpe6_3 = hpe6_2;
	 hpe6_2 = hpe6_1;
	 hpe6_1 = hpe6;
	 hpe6 = herror6;
	}
	// Averages the error values from the stored readings
	int hE3 = (int) ((hpe3 + hpe3_1 + hpe3_2 + hpe3_3 + hpe3_4)/5);

	int hE6 = (int) ((hpe6 + hpe6_1 + hpe6_2 + hpe6_3 + hpe6_4)/5);
	// Averages errors from the left side and right side
	int hEL = hE3;
	int hER = hE6;
	int hkp = 17 ; // random constant for the p controller
	// The P controller is the stop speed (128) + or - the 
	// product of the error and random constant
int hSR =  (int) (128 -(hkp*hER)); // 118 is still frozen?
int hSL =  (int) (128 +(hkp*hEL)); // 136 is still frozen
//these are to keep the speeds at acceptable values 0 - 255
	if (hSL > 255) hSL = 255; 
	if (hSL < 0) hSL = 0;
	if (hSR > 255) hSR = 255; 
	if (hSR < 0) hSR = 0;
	srv_left.SetPosition(hSL, 1); 
	srv_right.SetPosition(hSR, 1);
	Thread.sleep(100);
	System.out.println(hE3+"	"+"broken" +"	"+"broken"+"	"+ hE6);
	srv_left.SetPosition(128, 1); 
	srv_right.SetPosition(128, 1);
	break;
	
	
	}
	}
}
		}
	}
}

// broken sensors should be like if theyre spitting out values that are +/- 5 away from the sensor on the same side
// maybe before switching there should be like a broken test?
//if ((A <= C+5) || (A>=C-5)) this would say that A or C is broken

/* if i drop the p controller and just drive straight the 
whole time every sensor would give me  x x x x so then when
x x x y comes up i can do the considitions on every sensor
like 

if( ((A <= C+5) || (A>=C-5)) && ((A <= B+5) || (A>=B-5))&&
 ((A <= D+5) || (A>=D-5)) ) =! 1 

//would mean
 */